#include "robot_payload.h"
#include "jkutil/iniparser.h"
#include "log/zuclog.h"
#include "config.h"
#include <vector>

using namespace axisgroup;

RobotPayload::RobotPayload(uint32_t id) : id_(id) { reset(); }

RobotPayload::RobotPayload()
{
    id_ = 0;
    reset();
}

void RobotPayload::config_payload(Payload payload, std::string name)
{
    zuclog_info("PAYLOAD_INFO",
                "config_payload %d: from %.2f %.2f %.2f %.2f to %.2f %.2f %.2f %.2f name from %s to %s",
                id_,
                payload.m,
                payload.p[0],
                payload.p[1],
                payload.p[2],
                payload_.m,
                payload_.p[0],
                payload_.p[1],
                payload_.p[2],
                name_.c_str(),
                name.c_str());
    payload_ = payload;
    name_ = name;
    is_changed_ = true;
}

void RobotPayload::config_payload(Payload payload)
{
    zuclog_info("PAYLOAD_INFO",
                "config_payload %d: from %.2f %.2f %.2f %.2f to %.2f %.2f %.2f %.2f",
                id_,
                payload_.m,
                payload_.p[0],
                payload_.p[1],
                payload_.p[2],
                payload.m,
                payload.p[0],
                payload.p[1],
                payload.p[2]);
    payload_ = payload;
    is_changed_ = true;
}

void RobotPayload::reset() { name_ = "PAYLOAD" + std::to_string(id_); }

/**************************************************************************************************************************************/
axisgroup::RobotPayload RobotPayloadMgr::__robot_payload[RobotPayloadMgr::MAX_ROBOT_PAYLOAD_NUM];
std::unique_ptr<Util::IniParser> RobotPayloadMgr::ini_parser_;

std::string RobotPayload::get_payload_descript() const
{
    std::string res = "id : " + std::to_string(id_) + "m = " + std::to_string(payload_.m) + "p = " + std::to_string(payload_.p[0]) + "," +
                      std::to_string(payload_.p[1]) + ", " + std::to_string(payload_.p[2]);
    return res;
}

RobotPayload& RobotPayloadMgr::get_robot_payload(uint32_t id)
{
    if (id >= MAX_ROBOT_PAYLOAD_NUM)
    {
        return __robot_payload[0];
    }
    return __robot_payload[id];
}

int RobotPayloadMgr::init_rob_payload(const char* filename)
{
    ini_parser_.reset();
    ini_parser_ = std::make_unique<Util::IniParser>(filename);

    for (uint32_t i = 0; i < MAX_ROBOT_PAYLOAD_NUM; i++)
    {
        __robot_payload[i].id_ = i;
        std::string tag = "WORKPAYLOAD.PAYLOAD_" + std::to_string(i);
        RobotPayload::Payload payload = {0};
        std::vector<double> load(0, 4);
        ini_parser_->get(tag, load, {0, 0, 0, 0}, -10000, 10000);
        payload.m = load[0];
        payload.p[0] = load[1];
        payload.p[1] = load[2];
        payload.p[2] = load[3];

        std::string name;
        tag = "WORKPAYLOAD.NAME_" + std::to_string(i);
        ini_parser_->get(tag, name, "PAYLOAD" + std::to_string(i));
        zuclog_info("PAYLOAD_INFO", "Load Payload %d: %.2f %.2f %.2f %.2f name %s", i, payload.m, payload.p[0], payload.p[1], payload.p[2], name.c_str());
        __robot_payload[i].config_payload(payload, name);
        __robot_payload[i].is_changed_ = false;
    }
    return 0;
}

void RobotPayloadMgr::save()
{
    for (uint32_t i = 0; i < MAX_ROBOT_PAYLOAD_NUM; i++)
    {
        if (!__robot_payload[i].is_changed_)
        {
            continue;
        }
        std::string tag = "WORKPAYLOAD.PAYLOAD_" + std::to_string(i);
        RobotPayload::Payload payload = __robot_payload[i].payload();
        std::vector<double> load = {payload.m, payload.p[0], payload.p[1], payload.p[2]};
        ini_parser_->set(tag, load);
        tag = "WORKPAYLOAD.NAME_" + std::to_string(i);
        ini_parser_->set(tag, __robot_payload[i].name_);
        __robot_payload[i].is_changed_ = false;
    }
    ini_parser_->save_ini();
}